#include "slep.h"
#include "hr_time.h"
#include "Logistic.h"
#include "epph.h"


FUNVAL nnLogisticR(double *A, double* y, double z, OPTS opt){
	// Common for all 3 LogisticRm1, LogisticRm2, LogisticRm3
	int m,n,i,iterStep,bFlag;
	double m1,m2;
	double lambda, lambda_max,rsL2, c, cp, ccp, alpha, sc, fun_s, fun_x, fVal, fValp, l_sum, r_sum, gc, gamma, tp,t;
	//double *weight,*weighty,*b, *ATb,*x, *Ax, *Axp, *xp, *xxp, *s , *As, *aa, *bb, *prob, *funVal, *g, *v, *ValueL;
	double *x,*xp,*xxp,*Ax,*Axp, 
		*b, *weight, *weighty,
		*ATb,
		*s,*As,
		*aa,*bb,
		*prob,
		*g, *v,
		*ValueL,*funVal;
	FUNVAL funret;

	m = opt.m;
	n = opt.n;

	b = (double * ) malloc( sizeof(double) * m);
	ATb = (double * ) malloc( sizeof(double) * n);

	x = (double * ) malloc( sizeof(double) * n);
	xp = (double * ) malloc( sizeof(double) * n);
	xxp = (double * ) malloc( sizeof(double) * n);
	Ax = (double * ) malloc( sizeof(double) * m);
	Axp = (double * ) malloc( sizeof(double) * m);

	weight = (double * ) malloc( sizeof(double) * m);
	weighty = (double * ) malloc( sizeof(double) * m);

	s = (double * ) malloc( sizeof(double) * n);
	As = (double * ) malloc( sizeof(double) * m);

	aa = (double * ) malloc( sizeof(double) * m);
	bb = (double * ) malloc( sizeof(double) * m);
	prob = (double * ) malloc( sizeof(double) * m);

	g = (double * ) malloc( sizeof(double) * n);
	v = (double * ) malloc( sizeof(double) * n);

	funVal = (double * ) malloc( sizeof(double) * opt.maxIter);
	ValueL = (double * ) malloc( sizeof(double) * opt.maxIter);

	// Initialize normalization
	initNormalization(&opt,A);

	m1 = normalizeWeights(&opt,weight,y,b);
	m2 = 1.0 - m1;


	// Starting point
	rsL2 = opt.rsL2;
	if(opt.rFlag==0)
		lambda = z;
	else{
		if(z<0.0 || z>1.0){
			funret.errorCode = 1;
			return funret;
		}
		normalizedmv('T',&opt,A,b,ATb);
		lambda_max = idamax(n,ATb,1);
		lambda = z * lambda_max;
		rsL2 = rsL2 * lambda_max;
	}

	if( opt.init == 2){
		memset(x,0,sizeof(double)*n);
		c = 0.0;
	}
	else{
		if( hasGot(&opt,"x0"))
			dcopy(n,opt.x0,1,x,1);
		else
			memset(x,0,sizeof(double)*n);

		if( hasGot(&opt,"c0"))
			c = opt.c0;
		else
			c = 0.0;
	}

	normalizedmv('N',&opt,A,x,Ax);

	//------------------------------------------------
	bFlag = 0;
	gamma = 1.0/m + rsL2;

	// weighty = weight .* y;
	dsbmv('U',m,0,1.0,weight,1,y,1,0.0,weighty,1);

	// xp=x; Axp = Ax; xxp = zeros; cp = c; ccp=0;
	dcopy(n,x,1,xp,1);
	dcopy(m,Ax,1,Axp,1);
	memset(xxp,0,sizeof(double)*n);
	cp = c;
	ccp = 0.0;

	// Armijo Goldstein Line search scheme + accelerated gradient

	tp = 0.0;
	t = 1.0;
	for( iterStep = 0 ; iterStep < opt.maxIter ; iterStep++){
		alpha = (tp - 1.0)/t;

		//s = x + alpha * xxp;
		dcopy(n,x,1,s,1);
		daxpy(n,alpha,xxp,1,s,1);

		sc = c + alpha * ccp;

		//As = Ax + alpha* ( Ax - Axp);
		dcopy(m,Ax,1,As,1);
		dscal(m,1.0 + alpha,As,1);
		daxpy(m,-alpha,Axp,1,As,1);

		// aa = -y.* (As+ sc);
		dcopy(m,As,1,aa,1);
		daxpy(m,sc,oneVector,1,aa,1);
		dtbmv('U','N','N',m,0,y,1,aa,1);
		dscal(m,-1.0,aa,1);

		// fun_s= weight' * ( log( exp(-bb) +  exp(aa-bb) ) + bb )+...
        //    rsL2/2 * s'*s;
		fun_s = 0;
		for(i=0; i<m;i++){
			bb[i] = maxof2(aa[i],0.0);
			fun_s += weight[i] * ( log(exp(-bb[i])+exp(aa[i]-bb[i])) + bb[i]);
		}
		fun_s += rsL2 * ddot(n,s,1,s,1/2.0);

		//b = -weighty .* (1-prob);
		for(i=0;i<m;i++){
			prob[i] = 1.0/(1.0+exp(aa[i]));
			b[i] = - weighty[i] * (1.0 - prob[i]);
		}


		// gc =  sum(b); the gradient of c
		gc = ddot(m,b,1,oneVector,1);

		// g = ATb
		normalizedmv('T',&opt,A,b,g);

		// g = g + rsL2*s;
		daxpy(n,rsL2,s,1,g,1);

		// xp = x; Axp = Ax;
		// cp = c;
		dcopy(n,x,1,xp,1);
		dcopy(m,Ax,1,Axp,1);
		cp = c;

		for(;;){
			// v = s - g/gamma;
			dcopy(n,s,1,v,1);
			daxpy(n,-1.0/gamma,g,1,v,1);

			// c = sc - gc/gamma;
			c = sc - gc/gamma;

			for(i = 0 ; i <n ; i++ )
				x[i] = maxof2(v[i]- lambda/gamma , 0.0);

			dcopy(n,x,1,v,1);
			daxpy(n,-1.0,s,1,v,1);
			
			// Ax
			normalizedmv('N',&opt,A,x,Ax);

			// aa = -diag(y) * ( A*x + c)
			dcopy(m,Ax,1,aa,1);
			daxpy(m,c,oneVector,1,aa,1);
			dtbmv('U','N','N',m,0,y,1,aa,1);
			dscal(m,-1.0,aa,1);

			fun_x = 0.0;
			for( i = 0; i < m; i++ ){
				bb[i] = maxof2(aa[i],0.0);
				fun_x += weight[i]* ( log( exp(-bb[i]) + exp(aa[i] - bb[i])) + bb[i]);
			}
			fun_x += ddot(n,x,1,x,1) * rsL2/2.0;

			r_sum = (ddot(n,v,1,v,1) + (c-sc)*(c-sc))/2;
			l_sum = fun_x - fun_s - ddot(n,v,1,g,1) - (c-sc) * gc;

			if( r_sum <= 1.0e-20){
				bFlag=1;
				break;
			}

			if(l_sum <= r_sum * gamma)
				break;
			else
				gamma = maxof2(2*gamma, l_sum/r_sum);
		}

		tp = t;
		t = (1 + sqrt(4*t*t + 1))/2;

		// xxp = x - xp; ccp = c - cp;
		dcopy(n,x,1,xxp,1);
		daxpy(n,-1.0,xp,1,xxp,1);

		ccp = c - cp;

		fVal = fValp = funVal[iterStep] = fun_x;
		if( iterStep !=0)
			fValp = funVal[iterStep -1];

		funVal[iterStep] += lambda * ddot(n,x,1,oneVector,1);

		if(bFlag)
			break;

		if( terminationCondition(&opt,fVal,fValp,x,dnrm2(n,xxp,1),dnrm2(n,xp,1),iterStep) )
			break;

	}

	funret.errorCode = 0;
	funret.c = c;
	funret.funVal = funVal;
	funret.totIter = iterStep;
	strcpy(funret.type,"LogisticR");
	funret.x = x;
	funret.ValueL = ValueL;
	return funret;

}